
#include <Archaism/Math.h>
#include <cmath>
#include <cstring>

_BeginNamespace(Radix)
/*
============================================================
== Vector2
============================================================
*/
Vector2::Vector2()
	: x(0), y(0)
{}

Vector2::Vector2(real x, real y)
	: x(x), y(y)
{}

Vector2::Vector2(const real (&v)[2])
	: x(v[0]), y(v[1])
{}

Vector2::Vector2(const Vector2& v)
	: x(v.x), y(v.y)
{}

Vector2::~Vector2()
{}

Vector2& Vector2::operator=(const Vector2& v)
{
	x = v.x;
	y = v.y;
	return *this;
}

Vector2 Vector2::operator-()
{
	return Vector2(-x, -y);
}

Vector2 Vector2::operator+(const Vector2& v)
{
	return Vector2(x+v.x, y+v.y);
}

Vector2 Vector2::operator-(const Vector2& v)
{
	return Vector2(x-v.x, y-v.y);
}

Vector2 Vector2::operator*(const Vector2& v)
{
	return Vector2(x*v.x, y*v.y);
}

Vector2 Vector2::operator*(real t)
{
	return Vector2(x*t, y*t);
}

Vector2& Vector2::operator+=(const Vector2& v)
{
	x += v.x;
	y += v.y;
	return *this;
}

Vector2& Vector2::operator-=(const Vector2& v)
{
	x -= v.x;
	y -= v.y;
	return *this;
}

Vector2& Vector2::operator*=(const Vector2& v)
{
	x *= v.x;
	y *= v.y;
	return *this;
}

Vector2& Vector2::operator*=(real t)
{
	x *= t;
	y *= t;
	return *this;
}

bool Vector2::operator==(const Vector2& v)
{
	return _RealEqual(x, v.x)
		&& _RealEqual(y, v.y);
}

bool Vector2::operator!=(const Vector2& v)
{
	return !(_RealEqual(x, v.x)
		&& _RealEqual(y, v.y));
}

real Vector2::length()
{
	return sqrt(x*x + y*y);
}

Vector2& Vector2::normalize()
{
	real len = sqrt(x*x + y*y);
	len = (_RealEqual(len, 0.0f)) ? 1 : len;
	x /= len;
	y /= len;
	return *this;
}

/*
============================================================
== Vector3
============================================================
*/
Vector3::Vector3()
	: x(0), y(0), z(0)
{}

Vector3::Vector3(real x, real y, real z)
	: x(x), y(y), z(z)
{}

Vector3::Vector3(const real (&v)[3])
	: x(v[0]), y(v[1]), z(v[2])
{}

Vector3::Vector3(const Vector2& v)
	: x(v.x), y(v.y), z(1)
{}

Vector3::Vector3(const Vector3& v)
	: x(v.x), y(v.y), z(v.z)
{}

Vector3::~Vector3()
{}

Vector3& Vector3::operator=(const Vector3& v)
{
	x = v.x;
	y = v.y;
	z = v.z;
	return *this;
}

Vector3 Vector3::operator-()
{
	return Vector3(-x, -y, -z);
}

Vector3 Vector3::operator+(const Vector3& v)
{
	return Vector3(x+v.x, y+v.y, z+v.z);
}

Vector3 Vector3::operator-(const Vector3& v)
{
	return Vector3(x-v.x, y-v.y, z-v.z);
}

Vector3 Vector3::operator*(const Vector3& v)
{
	return Vector3(x*v.x, y*v.y, z*v.z);
}

Vector3 Vector3::operator*(real t)
{
	return Vector3(x*t, y*t, z*t);
}

Vector3& Vector3::operator+=(const Vector3& v)
{
	x += v.x;
	y += v.y;
	z += v.z;
	return *this;
}

Vector3& Vector3::operator-=(const Vector3& v)
{
	x -= v.x;
	y -= v.y;
	z -= v.z;
	return *this;
}

Vector3& Vector3::operator*=(const Vector3& v)
{
	x *= v.x;
	y *= v.y;
	z *= v.z;
	return *this;
}

Vector3& Vector3::operator*=(real t)
{
	x *= t;
	y *= t;
	z *= t;
	return *this;
}

bool Vector3::operator==(const Vector3& v)
{
	return _RealEqual(x, v.x)
		&& _RealEqual(y, v.y)
		&& _RealEqual(z, v.z);
}

bool Vector3::operator!=(const Vector3& v)
{
	return !(_RealEqual(x, v.x)
		&& _RealEqual(y, v.y)
		&& _RealEqual(z, v.z));
}

real Vector3::length()
{
	return sqrt(x*x + y*y + z*z);
}

Vector3& Vector3::normalize()
{
	real len = sqrt(x*x + y*y + z*z);
	len = (_RealEqual(len, 0.0f)) ? 1 : len;
	x /= len;
	y /= len;
	z /= len;
	return *this;
}

/*
============================================================
== Vector4
============================================================
*/
Vector4::Vector4()
	: x(0), y(0), z(0), w(0)
{}

Vector4::Vector4(real x, real y, real z, real w)
	: x(x), y(y), z(z), w(w)
{}

Vector4::Vector4(const real (&v)[4])
	: x(v[0]), y(v[1]), z(v[2]), w(v[3])
{}

Vector4::Vector4(const Vector3& v)
	: x(v.x), y(v.y), z(v.z), w(1)
{}

Vector4::Vector4(const Vector4& v)
	: x(v.x), y(v.y), z(v.z), w(v.w)
{}

Vector4::~Vector4()
{}

Vector4& Vector4::operator=(const Vector4& v)
{
	x = v.x;
	y = v.y;
	z = v.z;
	w = v.w;
	return *this;
}

Vector4 Vector4::operator-()
{
	return Vector4(-x, -y, -z, -w);
}

Vector4 Vector4::operator+(const Vector4& v)
{
	return Vector4(x+v.x, y+v.y, z+v.z, w+v.w);
}

Vector4 Vector4::operator-(const Vector4& v)
{
	return Vector4(x-v.x, y-v.y, z-v.z, w-v.w);
}

Vector4 Vector4::operator*(const Vector4& v)
{
	return Vector4(x*v.x, y*v.y, z*v.z, w*v.w);
}

Vector4 Vector4::operator*(real t)
{
	return Vector4(x*t, y*t, z*t, w*t);
}

Vector4& Vector4::operator+=(const Vector4& v)
{
	x += v.x;
	y += v.y;
	z += v.z;
	w += v.w;
	return *this;
}

Vector4& Vector4::operator-=(const Vector4& v)
{
	x -= v.x;
	y -= v.y;
	z -= v.z;
	w -= v.w;
	return *this;
}

Vector4& Vector4::operator*=(const Vector4& v)
{
	x *= v.x;
	y *= v.y;
	z *= v.z;
	w *= v.w;
	return *this;
}

Vector4& Vector4::operator*=(real t)
{
	x *= t;
	y *= t;
	z *= t;
	w *= t;
	return *this;
}

bool Vector4::operator==(const Vector4& v)
{
	return _RealEqual(x, v.x)
		&& _RealEqual(y, v.y)
		&& _RealEqual(z, v.z)
		&& _RealEqual(w, v.w);
}

bool Vector4::operator!=(const Vector4& v)
{
	return !(_RealEqual(x, v.x)
		&& _RealEqual(y, v.y)
		&& _RealEqual(z, v.z)
		&& _RealEqual(w, v.w));
}

/*
============================================================
== Matrix3x3
============================================================
*/
Matrix3x3::Matrix3x3()
{
	memset(value, 0, sizeof(value));
}

Matrix3x3::Matrix3x3(const real (&m)[3][3])
{
	memcpy(value, m, sizeof(value));
}

Matrix3x3::Matrix3x3(const Matrix3x3& m)
{
	memcpy(value, m.value, sizeof(value));
}

Matrix3x3::~Matrix3x3()
{}

Matrix3x3& Matrix3x3::operator=(const Matrix3x3& m)
{
	memcpy(value, m.value, sizeof(value));
	return *this;
}

Matrix3x3 Matrix3x3::operator-()
{
	real result[3][3];
	for(int i=0; i<3; i++)
	{
		for(int j=0; j<3; j++)
		{
			result[i][j] = -value[i][j];
		}
	}
	return Matrix3x3(result);
}

Matrix3x3 Matrix3x3::operator+(const Matrix3x3& m)
{
	real result[3][3];
	for(int i=0; i<3; i++)
	{
		for(int j=0; j<3; j++)
		{
			result[i][j] = value[i][j] + m.value[i][j];
		}
	}
	return Matrix3x3(result);;
}
Matrix3x3 Matrix3x3::operator-(const Matrix3x3& m)
{
	real result[3][3];
	for(int i=0; i<3; i++)
	{
		for(int j=0; j<3; j++)
		{
			result[i][j] = value[i][j] - m.value[i][j];
		}
	}
	return Matrix3x3(result);
}
Matrix3x3 Matrix3x3::operator*(const Matrix3x3& m)
{
	real result[3][3];
	for(int i=0; i<3; i++)
	{
		for(int j=0; j<3; j++)
		{
			result[i][j] = value[i][j] * m.value[i][j];
		}
	}
	return Matrix3x3(result);
}

Matrix3x3& Matrix3x3::operator+=(const Matrix3x3& m)
{
	for(int i=0; i<3; i++)
	{
		for(int j=0; j<3; j++)
		{
			value[i][j] += m.value[i][j];
		}
	}
	return *this;
}
Matrix3x3& Matrix3x3::operator-=(const Matrix3x3& m)
{
	for(int i=0; i<3; i++)
	{
		for(int j=0; j<3; j++)
		{
			value[i][j] -= m.value[i][j];
		}
	}
	return *this;
}
Matrix3x3& Matrix3x3::operator*=(const Matrix3x3& m)
{
	for(int i=0; i<3; i++)
	{
		for(int j=0; j<3; j++)
		{
			value[i][j] *= m.value[i][j];
		}
	}
	return *this;
}

bool Matrix3x3::operator==(const Matrix3x3& m)
{
	for(int i=0; i<3; i++)
	{
		for(int j=0; j<3; j++)
		{
			if(! _RealEqual(value[i][j], m.value[i][j]))
			{
				return false;
			}
		}
	}
	return true;
}

bool Matrix3x3::operator!=(const Matrix3x3& m)
{
	for(int i=0; i<3; i++)
	{
		for(int j=0; j<3; j++)
		{
			if(! _RealEqual(value[i][j], m.value[i][j]))
			{
				return true;
			}
		}
	}
	return false;
}

Matrix3x3& Matrix3x3::identity()
{
	memset(value, 0, sizeof(value));
	value[0][0] = 1.0f;
	value[1][1] = 1.0f;
	value[2][2] = 1.0f;
	return *this;
}

Matrix3x3& Matrix3x3::transpose()
{
	Matrix3x3 tmp(value);
	for(int i=1; i<3; i++)
	{
		for(int j=0; j<i; j++)
		{
			value[i][j] = tmp.value[j][i];
			value[j][i] = tmp.value[i][j];
		}
	}
	return *this;
}
/*
============================================================
== Matrix4x4
============================================================
*/
Matrix4x4::Matrix4x4()
{
	memset(value, 0, sizeof(value));
}

Matrix4x4::Matrix4x4(const real (&m)[4][4])
{
	memcpy(value, m, sizeof(value));
}

Matrix4x4::Matrix4x4(const Matrix3x3& m)
{
	memset(value, 0, sizeof(value));
	for(int i=0; i<3; i++)
	{
		for(int j=0; j<3; j++)
		{
			value[i][j] = m.value[i][j];
		}
	}
	value[3][3] = 1;
}

Matrix4x4::Matrix4x4(const Matrix4x4& m)
{
	memcpy(value, m.value, sizeof(value));
}

Matrix4x4::~Matrix4x4()
{}

Matrix4x4& Matrix4x4::operator=(const Matrix4x4& m)
{
	memcpy(value, m.value, sizeof(value));
	return *this;
}
	
Matrix4x4 Matrix4x4::operator-()
{
	real result[4][4];
	for(int i=0; i<4; i++)
	{
		for(int j=0; j<4; j++)
		{
			result[i][j] = -value[i][j];
		}
	}
	return Matrix4x4(result);
}

Matrix4x4 Matrix4x4::operator+(const Matrix4x4& m)
{
	real result[4][4];
	for(int i=0; i<4; i++)
	{
		for(int j=0; j<4; j++)
		{
			result[i][j] = value[i][j] + m.value[i][j];
		}
	}
	return Matrix4x4(result);;
}
Matrix4x4 Matrix4x4::operator-(const Matrix4x4& m)
{
	real result[4][4];
	for(int i=0; i<4; i++)
	{
		for(int j=0; j<4; j++)
		{
			result[i][j] = value[i][j] - m.value[i][j];
		}
	}
	return Matrix4x4(result);
}
Matrix4x4 Matrix4x4::operator*(const Matrix4x4& m)
{
	real result[4][4];
	for(int i=0; i<4; i++)
	{
		for(int j=0; j<4; j++)
		{
			result[i][j] = value[i][j] * m.value[i][j];
		}
	}
	return Matrix4x4(result);
}

Matrix4x4& Matrix4x4::operator+=(const Matrix4x4& m)
{
	for(int i=0; i<4; i++)
	{
		for(int j=0; j<4; j++)
		{
			value[i][j] += m.value[i][j];
		}
	}
	return *this;
}
Matrix4x4& Matrix4x4::operator-=(const Matrix4x4& m)
{
	for(int i=0; i<4; i++)
	{
		for(int j=0; j<4; j++)
		{
			value[i][j] -= m.value[i][j];
		}
	}
	return *this;
}
Matrix4x4& Matrix4x4::operator*=(const Matrix4x4& m)
{
	for(int i=0; i<4; i++)
	{
		for(int j=0; j<4; j++)
		{
			value[i][j] *= m.value[i][j];
		}
	}
	return *this;
}

bool Matrix4x4::operator==(const Matrix4x4& m)
{
	for(int i=0; i<4; i++)
	{
		for(int j=0; j<4; j++)
		{
			if(! _RealEqual(value[i][j], m.value[i][j]))
			{
				return false;
			}
		}
	}
	return true;
}

bool Matrix4x4::operator!=(const Matrix4x4& m)
{
	for(int i=0; i<4; i++)
	{
		for(int j=0; j<4; j++)
		{
			if(! _RealEqual(value[i][j], m.value[i][j]))
			{
				return true;
			}
		}
	}
	return false;
}
	
Matrix4x4& Matrix4x4::identity()
{
	memset(value, 0, sizeof(value));
	value[0][0] = 1.0f;
	value[1][1] = 1.0f;
	value[2][2] = 1.0f;
	value[3][3] = 1.0f;
	return *this;
}

Matrix4x4& Matrix4x4::transpose()
{
	Matrix4x4 tmp(value);
	for(int i=1; i<4; i++)
	{
		for(int j=0; j<i; j++)
		{
			value[i][j] = tmp.value[j][i];
			value[j][i] = tmp.value[i][j];
		}
	}
	return *this;
}

/*
============================================================
== Quaternion
============================================================
*/

/*
============================================================
== Ray
============================================================
*/
Ray::Ray()
	:mOrigin(0, 0, 0),
	mDirection(0, 1, 0)
{}

Ray::Ray(const Vector3& origin, const Vector3& direction)
	:mOrigin(origin),
	mDirection(direction)
{}

Ray::Ray(const Ray& ray)
	:mOrigin(ray.mOrigin),
	mDirection(ray.mDirection)
{}

Vector3 Ray::getPoint(real t)
{
	mDirection.normalize();
	return Vector3(mOrigin + mDirection * t);
}
/*
============================================================
== Math
============================================================
*/
const real Math::mskEpsilon4 = 0.0001f;
const real Math::mskEpsilon5 = 0.00001f;
const real Math::mskEpsilon6 = 0.000001f;
const real Math::mskPi = 3.1415926f;

real Math::degreeToRadian(real degree)
{
	static const real degreeUnit = mskPi / 180.0f;
	return degree * degreeUnit;
}

real Math::radianToDegree(real radian)
{
	static const real radianUnit = 180.0f / mskPi;
	return radian * radianUnit;
}

real Math::clamp(real x, real min, real max)
{
	x = x < min ? min : x;
	x = x > max ? max : x;
	return x;
}

real Math::saturate(real x)
{
	return Math::clamp(x, 0.0f, 1.0f);
}

real Math::dot(const Vector2& v1, const Vector2& v2)
{
	return v1.x*v2.x + v1.y*v2.y;
}

real Math::dot(const Vector3& v1, const Vector3& v2)
{
	return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
}

real Math::dot(const Vector4& v1, const Vector4& v2)
{
	return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z + v1.w*v2.w;
}

Vector3 Math::cross(const Vector3& v1, const Vector3& v2)
{
	//( y1z2 - z1y2 , z1x2 - x1z2 , x1y2 - y1x2 ) 
	return Vector3(v1.y*v2.z - v1.z*v2.y,  v1.z*v2.x - v1.x*v2.z,  v1.x*v2.y - v1.y*v2.x);
}

void Math::transform(Vector3& result, const Vector3& v, const Matrix3x3& m)
{
	result.x = v.x*m.value[0][0] + v.y*m.value[1][0] + v.z*m.value[2][0];
	result.y = v.x*m.value[0][1] + v.y*m.value[1][1] + v.z*m.value[2][1];
	result.z = v.x*m.value[0][2] + v.y*m.value[1][2] + v.z*m.value[2][2];
}

void Math::transform(Vector4& result, const Vector4& v, const Matrix4x4& m)
{
	result.x = v.x*m.value[0][0] + v.y*m.value[1][0] + v.z*m.value[2][0] + v.w*m.value[3][0];
	result.y = v.x*m.value[0][1] + v.y*m.value[1][1] + v.z*m.value[2][1] + v.w*m.value[3][1];
	result.z = v.x*m.value[0][2] + v.y*m.value[1][2] + v.z*m.value[2][2] + v.w*m.value[3][2];
	result.w = v.x*m.value[0][3] + v.y*m.value[1][3] + v.z*m.value[2][3] + v.w*m.value[3][3];
}

void Math::transform(Matrix3x3& result, const Matrix3x3& m1, const Matrix3x3& m2)
{
	for(int i=0; i<3; i++)
	{
		for(int j=0; j<3; j++)
		{
			for(int k=0; k<3; k++)
			{
				result.value[i][j] += m1.value[i][k]*m2.value[k][j];
			}
		}
	}
}

void Math::transform(Matrix4x4& result, const Matrix4x4& m1, const Matrix3x3& m2)
{
	for(int i=0; i<4; i++)
	{
		for(int j=0; j<4; j++)
		{
			for(int k=0; k<4; k++)
			{
				result.value[i][j] += m1.value[i][k]*m2.value[k][j];
			}
		}
	}
}

Matrix3x3 Math::matrixTranslation(const Vector2& dir)
{
	Matrix3x3 m;
	m.identity();
	m.value[2][0] = dir.x;
	m.value[2][1] = dir.y;
	return m;
}

Matrix4x4 Math::matrixTranslation(const Vector3& dir)
{
	Matrix4x4 m;
	m.identity();
	m.value[3][0] = dir.x;
	m.value[3][1] = dir.y;
	m.value[3][2] = dir.z;
	return m;
}
/*
| 1,  0,  0|     | cosA,  sinA,  0|     | 1,  0,  0|
| 0,  1,  0|  X  |-sinA,  cosA,  0|  X  | 0,  1,  0|
|-x, -y,  1|     |    0,     0,  1|     | x,  y,  1|
*/
Matrix3x3 Math::matrixRotation(const Vector2& axis, real angle)
{
	Matrix3x3 m;
	m.identity();
	real x = axis.x;
	real y = axis.y;
	real sinAngle = sin(angle);
	real cosAngle = cos(angle);
	m.value[0][0] = cosAngle;
	m.value[0][1] = sinAngle;
	m.value[0][2] = 0.0f;
	m.value[1][0] = -sinAngle;
	m.value[1][1] = cosAngle;
	m.value[1][2] = 0.0f;
	m.value[2][0] = x * (1.0f - cosAngle) + y * sinAngle;
	m.value[2][1] = y * (1.0f - cosAngle) - x * cosAngle;
	m.value[2][2] = 1.0f;
	return m;
}

Matrix4x4 Math::matrixRotation(const Vector3& axis, real angle)
{
	Matrix4x4 m;
	m.identity();
	real x = axis.x;
	real y = axis.y;
	real z = axis.z;
	real sinAngle = sin(angle);
	real cosAngle = cos(angle);
	m.value[0][0] = (x * x ) * ( 1.0f - cosAngle ) + cosAngle;
    m.value[1][0] = (x * y ) * ( 1.0f - cosAngle ) - (z * sinAngle);
    m.value[2][0] = (x * z ) * ( 1.0f - cosAngle ) + (y * sinAngle);
    m.value[0][1] = (y * x ) * ( 1.0f - cosAngle ) + (z * sinAngle);
    m.value[1][1] = (y * y ) * ( 1.0f - cosAngle ) + cosAngle ;
    m.value[2][1] = (y * z ) * ( 1.0f - cosAngle ) - (x * sinAngle);
    m.value[0][2] = (z * x ) * ( 1.0f - cosAngle ) - (y * sinAngle);
    m.value[1][2] = (z * y ) * ( 1.0f - cosAngle ) + (x * sinAngle);
    m.value[2][2] = (z * z ) * ( 1.0f - cosAngle ) + cosAngle;
	return m;
}

Matrix3x3 Math::matrixScaling(const Vector2& scale)
{
	Matrix3x3 m;
	m.identity();
	m.value[0][0] = scale.x;
	m.value[1][1] = scale.y;
	return m;
}

Matrix4x4 Math::matrixScaling(const Vector3& scale)
{
	Matrix4x4 m;
	m.identity();
	m.value[0][0] = scale.x;
	m.value[1][1] = scale.y;
	m.value[2][2] = scale.z;
	return m;
}

_EndNamespace(Radix)
